#include <errno.h>
#include <math.h>
#include <pthread.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/mman.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <time.h>
#include <unistd.h>

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <fcntl.h>

#include "ecrt.h"
#include "./tinyxml2.h"

#include "pid.h"
#include "filter.h"

#define false 0
#define true 1
#define RUNTIME_MS 60000 // 60s
#define ETHERCAT_STATUS_OP 0x08
#define STATUS_SERVO_ENABLE_BIT (0x04)

// 控制周期 ms 转换
#define PERIOD_NS   (1 * 1000 * 1000)   // 1 ms = 1,000,000 ns
#define PERIOD_MS   (PERIOD_NS / 1000000)
#define PERIOD_S    (PERIOD_NS / 1000000000.0)

// 运行模式
#define CSP_MODE 8  // 位置模式

#define Encoder_resolution 131072      // 编码器分辨率 2^17
#define Encoder_resolution_ms (Encoder_resolution / 1000)  // 编码器分辨率 转为ms
#define X_AXIS_REDUCTION_RATIO 10   // x轴减速比
#define Y_AXIS_REDUCTION_RATIO 1    // y轴减速比

// UDP 控制端口
#define UDP_PORT 5005

// master status
typedef enum _SysWorkingStatus
{
    SYS_WORKING_POWER_ON,
    SYS_WORKING_SAFE_MODE,
    SYS_WORKING_ORIGIN_MODE,
    SYS_WORKING_OP_MODE,
    SYS_WORKING_LINK_DOWN,
    SYS_WORKING_IDLE_STATUS // 系统空闲
} SysWorkingStatus;
typedef struct _GSysRunningParm
{
    SysWorkingStatus m_gWorkStatus;
} GSysRunningParm;
GSysRunningParm gSysRunning;

pthread_t InterpolationTask;
int run = 1;

// Axis Status
static int x_ready_op = 0, y_ready_op = 0;
static int x_ready_origin = 0, y_ready_origin = 0;
// curpos_* 表示编码器当前脉冲读数（由 EtherCAT 输入得到）
static int curpos_x = 0, curpos_y = 0;

static pthread_mutex_t pos_mutex = PTHREAD_MUTEX_INITIALIZER;

// === PID & 滤波器 ===
PID pid_x(1.0, 0.0, 0.0, -5.0, 5.0);
PID pid_y(1.0, 0.0, 0.0, -5.0, 5.0);
LowPassFilter filter_x(0.2);
LowPassFilter filter_y(0.2);

// 线程间命令共享结构
static pthread_mutex_t cmd_mutex = PTHREAD_MUTEX_INITIALIZER;
static double recv_pan_cmd = 0.0;   // 来自 UDP 的期望 pan（角度，度）
static double recv_tilt_cmd = 0.0;  // 来自 UDP 的期望 tilt（角度，度）
static double recv_speed_cmd = 100.0; // 来自 UDP 的速度字段
static int cmd_available = 0; // 标志：是否收到过命令（实时线程会读取并使用）

#define CLOCK_TO_USE CLOCK_REALTIME
#define NSEC_PER_SEC (1000000000L)
#define TIMESPEC2NS(T) ((uint64_t)(T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)

static int64_t system_time_base = 0LL;
// 获取当前系统时间
uint64_t system_time_ns(void)
{
    struct timespec rt_time;
    clock_gettime(CLOCK_TO_USE, &rt_time);
    uint64_t time = TIMESPEC2NS(rt_time);
    return time - system_time_base;
}

/****************************************************************************/
// EtherCAT variables
ec_master_t *master = NULL;
static ec_master_state_t master_state = {};
static ec_domain_t *domainServoInput = NULL;
static ec_domain_state_t domainServoInput_state = {};
static ec_domain_t *domainServoOutput = NULL;
static ec_domain_state_t domainServoOutput_state = {};
static uint8_t *domainOutput_pd = NULL;
static uint8_t *domainInput_pd = NULL;

// 两套从站配置
static ec_slave_config_t *sc_x_axis = NULL;  // X轴配置
static ec_slave_config_t *sc_y_axis = NULL;  // Y轴配置
static ec_slave_config_state_t sc_x_axis_state, sc_y_axis_state;

// PDO 索引
static unsigned int cntlwd_x, targetpos_x, operation_mode_x, origin_mode_x;
static unsigned int statwd_x, actpos_x, modes_of_operation_display_x;

static unsigned int cntlwd_y, targetpos_y, operation_mode_y, origin_mode_y;
static unsigned int statwd_y, actpos_y, modes_of_operation_display_y;

static unsigned int cur_statuswd_x, cur_mode_x;
static unsigned int cur_statuswd_y, cur_mode_y;

/****************************************************************************/
/* servo pdo regs and config */
/* pdo 注册表和配置 */
#define X_AXIS_POS 0, 0  // X轴位置
#define Y_AXIS_POS 0, 1  // Y轴位置
#define X5S 0x00000766, 0x00010000
#define KS100E 0x00000100, 0x00000100

// 为两套从站定义独立的PDO偏移量
ec_pdo_entry_reg_t domainServoOutput_regs[] = {
    // X轴
    {X_AXIS_POS, X5S, 0x6040, 0x00, &cntlwd_x, NULL},
    {X_AXIS_POS, X5S, 0x607a, 0x00, &targetpos_x, NULL},
    {X_AXIS_POS, X5S, 0x6060, 0x00, &operation_mode_x, NULL},
    {X_AXIS_POS, X5S, 0x6098, 0x00, &origin_mode_x, NULL},

    // Y轴
    {Y_AXIS_POS, KS100E, 0x6040, 0x00, &cntlwd_y, NULL},
    {Y_AXIS_POS, KS100E, 0x607a, 0x00, &targetpos_y, NULL},
    {Y_AXIS_POS, KS100E, 0x6060, 0x00, &operation_mode_y, NULL},
    {Y_AXIS_POS, KS100E, 0x6098, 0x00, &origin_mode_y, NULL},
    {}};

ec_pdo_entry_reg_t domainServoInput_regs[] = {
    // X轴
    {X_AXIS_POS, X5S, 0x6041, 0x00, &statwd_x, NULL},
    {X_AXIS_POS, X5S, 0x6064, 0x00, &actpos_x, NULL},
    {X_AXIS_POS, X5S, 0x6061, 0x00, &modes_of_operation_display_x, NULL},

    // Y轴
    {Y_AXIS_POS, KS100E, 0x6041, 0x00, &statwd_y, NULL},
    {Y_AXIS_POS, KS100E, 0x6064, 0x00, &actpos_y, NULL},
    {Y_AXIS_POS, KS100E, 0x6061, 0x00, &modes_of_operation_display_y, NULL},
    {}};

ec_pdo_entry_info_t servo_pdo_entries[] = {
    {0x6040, 0x00, 16}, /* Controlword */
    {0x6060, 0x00, 8},  /* Modes of operation */
    {0x6098, 0x00, 8},  /* 原点模式 */
    {0x607a, 0x00, 32}, /* Target position */
    {0x60b8, 0x00, 16}, /* Touch probe function */
    {0x603f, 0x00, 16}, /* Error code */
    {0x6041, 0x00, 16}, /* Statusword */
    {0x6061, 0x00, 8},  /* Modes of operation display */
    {0x6064, 0x00, 32}, /* Position actual value */
    {0x60b9, 0x00, 16}, /* Touch probe status */
    {0x60ba, 0x00, 32}, /* Touch probe pos1 pos value */
    {0x60f4, 0x00, 32}, /* Following error actual value */
    {0x60fd, 0x00, 32}, /* Digital inputs */
};
ec_pdo_info_t servo_pdos[] = {
    {0x1600, 5, servo_pdo_entries + 0}, /* Receive PDO mapping 1 */
    {0x1a00, 8, servo_pdo_entries + 5}, /* Transmit PDO mapping 1 */
};
ec_sync_info_t servo_syncs[] = {{0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
                                {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
                                {2, EC_DIR_OUTPUT, 1, servo_pdos + 0, EC_WD_ENABLE},
                                {3, EC_DIR_INPUT, 1, servo_pdos + 1, EC_WD_DISABLE},
                                {0xff}};

/****************************************************************************/
int ConfigPDO()
{
    /********************/
    printf("Configuring PDOs...\n");
    domainServoOutput = ecrt_master_create_domain(master);
    if (!domainServoOutput) {
        return -1;
    }
    domainServoInput = ecrt_master_create_domain(master);
    if (!domainServoInput) {
        return -1;
    }
    /********************/
    printf("Creating slave configurations...\n");
    // 创建X轴配置
    sc_x_axis = ecrt_master_slave_config(master, X_AXIS_POS, X5S);
    if (!sc_x_axis) {
        fprintf(stderr, "Failed to get X axis slave configuration.\n");
        return -1;
    }

    // 创建Y轴配置
    sc_y_axis = ecrt_master_slave_config(master, Y_AXIS_POS, KS100E);
    if (!sc_y_axis) {
        fprintf(stderr, "Failed to get Y axis slave configuration.\n");
        return -1;
    }
    /********************/
    // 配置X轴PDO
    if (ecrt_slave_config_pdos(sc_x_axis, EC_END, servo_syncs)) {
        fprintf(stderr, "Failed to configure X axis PDOs.\n");
        return -1;
    }

    // 配置Y轴PDO
    if (ecrt_slave_config_pdos(sc_y_axis, EC_END, servo_syncs)) {
        fprintf(stderr, "Failed to configure Y axis PDOs.\n");
        return -1;
    }
    /********************/
    if (ecrt_domain_reg_pdo_entry_list(domainServoOutput, domainServoOutput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    if (ecrt_domain_reg_pdo_entry_list(domainServoInput, domainServoInput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }

    // 此处SDO映射有问题
    fprintf(stderr, "Creating SDO requests...\n");
    // ecrt_slave_config_sdo8(sc_x_axis, 0x6060, 0, CSP_MODE);
    // ecrt_slave_config_sdo8(sc_y_axis, 0x6060, 0, CSP_MODE);
    return 0;
}

/*****************************************************************************
 * Realtime task
 ****************************************************************************/
void rt_check_domain_state(void)
{
    ec_domain_state_t ds = {};
    ec_domain_state_t ds1 = {};
    // domainServoInput
    ecrt_domain_state(domainServoInput, &ds);
    if (ds.working_counter != domainServoInput_state.working_counter) {
        printf("domainServoInput: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoInput_state.wc_state) {
        printf("domainServoInput: State %u.\n", ds.wc_state);
    }
    domainServoInput_state = ds;
    // domainServoOutput
    ecrt_domain_state(domainServoOutput, &ds1);
    if (ds1.working_counter != domainServoOutput_state.working_counter) {
        printf("domainServoOutput: WC %u.\n", ds1.working_counter);
    }
    if (ds1.wc_state != domainServoOutput_state.wc_state) {
        printf("domainServoOutput: State %u.\n", ds1.wc_state);
    }
    domainServoOutput_state = ds1;
}
void rt_check_master_state(void)
{
    ec_master_state_t ms;
    ecrt_master_state(master, &ms);
    if (ms.slaves_responding != master_state.slaves_responding) {
        printf("%u slave(s).\n", ms.slaves_responding);
    }
    if (ms.al_states != master_state.al_states) {
        printf("AL states: 0x%02X.\n", ms.al_states);
    }
    if (ms.link_up != master_state.link_up) {
        printf("Link is %s.\n", ms.link_up ? "up" : "down");
    }
    master_state = ms;
}
void check_slave_config_states(void)
{
    ec_slave_config_state_t s;

    // 检查X轴状态
    ecrt_slave_config_state(sc_x_axis, &s);
    if (s.al_state != sc_x_axis_state.al_state)
        printf("X axis: State 0x%02X.\n", s.al_state);
    if (s.online != sc_x_axis_state.online)
        printf("X axis: %s.\n", s.online ? "online" : "offline");
    if (s.operational != sc_x_axis_state.operational)
        printf("X axis: %soperational.\n", s.operational ? "" : "Not ");
    sc_x_axis_state = s;

    // 检查Y轴状态
    ecrt_slave_config_state(sc_y_axis, &s);
    if (s.al_state != sc_y_axis_state.al_state)
        printf("Y axis: State 0x%02X.\n", s.al_state);
    if (s.online != sc_y_axis_state.online)
        printf("Y axis: %s.\n", s.online ? "online" : "offline");
    if (s.operational != sc_y_axis_state.operational)
        printf("Y axis: %soperational.\n", s.operational ? "" : "Not ");
    sc_y_axis_state = s;
}
void ReleaseMaster()
{
    if (master) {
        printf("End of Program, release master\n");
        ecrt_release_master(master);
        master = NULL;
    }
}
int ActivateMaster()
{
    int ret;
    printf("Requesting master...\n");
    if (master)
        return 0;
    master = ecrt_request_master(0);
    if (!master) {
        return -1;
    }
    ConfigPDO();
    // 配置两套从站的SYNC信号
    ecrt_slave_config_dc(sc_x_axis, 0x0300, PERIOD_NS, 0, 0, 0);
    ecrt_slave_config_dc(sc_y_axis, 0x0300, PERIOD_NS, 0, 0, 0);

    ecrt_master_application_time(master, system_time_ns());
    ret = ecrt_master_select_reference_clock(master, NULL);
    if (ret < 0) {
        fprintf(stderr, "Failed to select reference clock: %s\n", strerror(-ret));
        return ret;
    }
    /********************/
    printf("Activating master...\n");
    if (ecrt_master_activate(master)) {
        printf("Activating master...failed\n");
        return -1;
    }
    /********************/
    if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
        fprintf(stderr, "Failed to get domain data pointer.\n");
        return -1;
    }
    if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
        fprintf(stderr, "Failed to get domain data pointer.\n");
        return -1;
    }
    printf("Activating master...success\n");
    return 0;
}

void AxisProcessESM(int CurWorkStatus)
{
    // X轴状态处理
    if ((cur_statuswd_x & 0x004f) == 0x0040) {
        EC_WRITE_U16(domainOutput_pd + cntlwd_x, 0x06);
    } else if ((cur_statuswd_x & 0x006f) == 0x0021) {
        EC_WRITE_U16(domainOutput_pd + cntlwd_x, 0x07);
    } else if ((cur_statuswd_x & 0x006f) == 0x023) {
        EC_WRITE_U16(domainOutput_pd + cntlwd_x, 0x0F);
        pthread_mutex_lock(&pos_mutex);
        curpos_x = EC_READ_S32(domainInput_pd + actpos_x);
        pthread_mutex_unlock(&pos_mutex);
    } else if ((cur_statuswd_x & 0x006f) == 0x0027) {
        EC_WRITE_U16(domainOutput_pd + cntlwd_x, 0x001f);
        pthread_mutex_lock(&pos_mutex);
        curpos_x = EC_READ_S32(domainInput_pd + actpos_x);
        pthread_mutex_unlock(&pos_mutex);
        EC_WRITE_S32(domainOutput_pd + targetpos_x, curpos_x);  // 当前位置设置为目标位置
        if(CurWorkStatus == SYS_WORKING_ORIGIN_MODE) {
            x_ready_origin = 1;
        }else if(CurWorkStatus == SYS_WORKING_OP_MODE) {
            x_ready_op = 1;
        }
    }

    // Y轴状态处理
    if ((cur_statuswd_y & 0x004f) == 0x0040) {
        EC_WRITE_U16(domainOutput_pd + cntlwd_y, 0x06);
    } else if ((cur_statuswd_y & 0x006f) == 0x0021) {
        EC_WRITE_U16(domainOutput_pd + cntlwd_y, 0x07);
    } else if ((cur_statuswd_y & 0x006f) == 0x023) {
        EC_WRITE_U16(domainOutput_pd + cntlwd_y, 0x0F);
        pthread_mutex_lock(&pos_mutex);
        curpos_y = EC_READ_S32(domainInput_pd + actpos_y);
        pthread_mutex_unlock(&pos_mutex);
    } else if ((cur_statuswd_y & 0x006f) == 0x0027) {
        EC_WRITE_U16(domainOutput_pd + cntlwd_y, 0x001f);
        pthread_mutex_lock(&pos_mutex);
        curpos_y = EC_READ_S32(domainInput_pd + actpos_y);
        pthread_mutex_unlock(&pos_mutex);
        EC_WRITE_S32(domainOutput_pd + targetpos_y, curpos_y);  // 当前位置设置为目标位置
        if(CurWorkStatus == SYS_WORKING_ORIGIN_MODE) {
            y_ready_origin = 1;
        }else if(CurWorkStatus == SYS_WORKING_OP_MODE) {
            y_ready_op = 1;
        }
    }
}

/****************************************************************************/
/*
    角度 -> 脉冲 : 返回相对于编码器的脉冲差(pulses)
*/
static inline int angle_deg_to_pulses(double angle_deg, int axis)
{
    // 每圈脉冲数：2^17 = 131072

    // 每度脉冲数 (pulses/deg) = 131072 / 360
    double pulses_per_degree = (double)Encoder_resolution / 360.0;

    // 目标总脉冲数 (pulses) = 角度 * 每度脉冲数
    double pulses = angle_deg * pulses_per_degree;

    // 减速比换算
    if(axis == 0)
        pulses *= X_AXIS_REDUCTION_RATIO;
    else
        pulses *= Y_AXIS_REDUCTION_RATIO;

    return (int)round(pulses);
}

// 脉冲 -> 角度（度）
static inline double pulses_to_angle_deg(int pulses, int axis)
{
    double deg = (double)pulses / Encoder_resolution * 360.0;
    if (axis == 0) deg /= X_AXIS_REDUCTION_RATIO;
    else deg /= Y_AXIS_REDUCTION_RATIO;
    return deg;
}

/*
    MoveRelativeImmediate: 直接把目标位置设置为 curpos + delta_pulses
    axis: 0 = X (pan), 1 = Y (tilt)
    angle_deg: 相对角度（度），正负表示方向
    speed_deg_s: 角速度
*/
void MoveRelativeImmediate(double angle_deg, double speed_deg_s, int axis)
{
    int pulses = angle_deg_to_pulses(angle_deg, axis);

    pthread_mutex_lock(&pos_mutex);
    if (axis == 0) { // X
        int cur = curpos_x;
        int target = cur + pulses;
        printf("[CMD] MoveRelativeImmediate axis=X angle=%.3f deg pulses=%d cur=%d target=%d speed=%.1f\n",
               angle_deg, pulses, cur, target, speed_deg_s);
        EC_WRITE_S32(domainOutput_pd + targetpos_x, target);
        curpos_x = target;
    } else { // Y
        int cur = curpos_y;
        int target = cur + pulses;
        printf("[CMD] MoveRelativeImmediate axis=Y angle=%.3f deg pulses=%d cur=%d target=%d speed=%.1f\n",
               angle_deg, pulses, cur, target, speed_deg_s);
        EC_WRITE_S32(domainOutput_pd + targetpos_y, target);
        curpos_y = target;
    }
    pthread_mutex_unlock(&pos_mutex);
}

void StepMoveTest(double angle_deg, double speed_deg_s, int axis)
{
    static int count = 0;
    if (count < 60) {
        angle_deg = 1.0;
    } else if (count < 120) {
        angle_deg = -1.0;
    } else {
        count = 0;
    }
    count++;

    int pulses = angle_deg_to_pulses(angle_deg, axis);

    pthread_mutex_lock(&pos_mutex);
    if (axis == 0) { // X
        int cur = curpos_x;
        int target = cur + pulses;
        printf("[CMD] axis=X angle=%.3f deg pulses=%d cur=%d target=%d speed=%.1f\n",
               angle_deg, pulses, cur, target, speed_deg_s);
        EC_WRITE_S32(domainOutput_pd + targetpos_x, target);
        curpos_x = target;
    } else { // Y
        int cur = curpos_y;
        int target = cur + pulses;
        printf("[CMD] axis=Y angle=%.3f deg pulses=%d cur=%d target=%d speed=%.1f\n",
               angle_deg, pulses, cur, target, speed_deg_s);
        EC_WRITE_S32(domainOutput_pd + targetpos_y, target);
        curpos_y = target;
    }
    pthread_mutex_unlock(&pos_mutex);
}

/****************************************************************************/
/* UDP 接收线程：监听 MOVE <pan_deg> <tilt_deg> <speed>
   仅把解析出的命令写入共享变量，由实时线程读取执行。
*/
void *udp_thread_func(void *arg)
{
    int sockfd;
    struct sockaddr_in servaddr, cliaddr;
    char buffer[256];

    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if (sockfd < 0) {
        perror("udp socket create failed");
        return NULL;
    }

    memset(&servaddr, 0, sizeof(servaddr));
    servaddr.sin_family = AF_INET;
    servaddr.sin_addr.s_addr = htonl(INADDR_ANY);
    servaddr.sin_port = htons(UDP_PORT);

    if (bind(sockfd, (struct sockaddr *)&servaddr, sizeof(servaddr)) < 0) {
        perror("udp bind failed");
        close(sockfd);
        return NULL;
    }
    // 设置非阻塞接收，避免阻塞实时线程启动
    int flags = fcntl(sockfd, F_GETFL, 0);
    fcntl(sockfd, F_SETFL, flags | O_NONBLOCK);

    printf("[UDP] Listening on port %d for MOVE commands\n", UDP_PORT);

    while (run) {
        socklen_t len = sizeof(cliaddr);
        ssize_t n = recvfrom(sockfd, buffer, sizeof(buffer)-1, 0, (struct sockaddr *)&cliaddr, &len);
        if (n > 0) {
            buffer[n] = '\0';
            // 解析
            double pan_cmd=0.0, tilt_cmd=0.0, speed=100.0;
            if (sscanf(buffer, "MOVE %lf %lf %lf", &pan_cmd, &tilt_cmd, &speed) >= 2) {
                // 限位到 -180..180
                if (pan_cmd > 180.0) pan_cmd = 180.0;
                if (pan_cmd < -180.0) pan_cmd = -180.0;
                if (tilt_cmd > 180.0) tilt_cmd = 180.0;
                if (tilt_cmd < -180.0) tilt_cmd = -180.0;

                // 打印来源
                char cli_ip[INET_ADDRSTRLEN];
                inet_ntop(AF_INET, &cliaddr.sin_addr, cli_ip, sizeof(cli_ip));
                printf("[UDP] recv from %s:%d -> %s\n", cli_ip, ntohs(cliaddr.sin_port), buffer);

                // 把命令写入共享变量（线程安全）
                pthread_mutex_lock(&cmd_mutex);
                recv_pan_cmd = pan_cmd;
                recv_tilt_cmd = tilt_cmd;
                recv_speed_cmd = speed;
                cmd_available = 1;
                pthread_mutex_unlock(&cmd_mutex);

            } else {
                printf("[UDP] unknown command: %s\n", buffer);
            }
        }
        usleep(20000); // 20ms sleep to avoid busy loop
    }
    close(sockfd);
    printf("[UDP] udp thread exit\n");
    return NULL;
}

/****************************************************************************/
void DriverEtherCAT()
{
    static int i = 0;
    // 处于刚开机(需要等待其他操作完成),返回等待下次周期
    if (gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON)
        return;

    static int cycle_counter = 0;
    cycle_counter++;
    int runtime_tick = 0;
    if (PERIOD_MS > 0) {
        runtime_tick = RUNTIME_MS / PERIOD_MS;
    } else {
        runtime_tick = 10000;
    }
    // receive EtherCAT frames
    ecrt_master_receive(master);
    ecrt_domain_process(domainServoOutput);
    ecrt_domain_process(domainServoInput);
    rt_check_domain_state();
    if ((cycle_counter % 500) == 0) {
        rt_check_master_state();
        check_slave_config_states();
    }

    /*
        状态机操作
    */
    switch (gSysRunning.m_gWorkStatus) {
    case SYS_WORKING_SAFE_MODE:
        rt_check_master_state();
        check_slave_config_states();
        if ((master_state.al_states & ETHERCAT_STATUS_OP)) {
            int tmp = true;
            if (sc_x_axis_state.al_state != ETHERCAT_STATUS_OP ||
                sc_y_axis_state.al_state != ETHERCAT_STATUS_OP) {
                tmp = false;
                break;
            }
            if (tmp) {
                gSysRunning.m_gWorkStatus = SYS_WORKING_ORIGIN_MODE;
                printf(" igh_ESM SYS_WORKING_ORIGIN_MODE \n");
                // X轴错误复位
                EC_WRITE_U16(domainOutput_pd + cntlwd_x, 0x80);
                // Y轴错误复位
                EC_WRITE_U16(domainOutput_pd + cntlwd_y, 0x80);
            }
        }
        break;
    case SYS_WORKING_ORIGIN_MODE:
        EC_WRITE_U8(domainOutput_pd + operation_mode_x, 6);
        EC_WRITE_U8(domainOutput_pd + origin_mode_x, 35);
        cur_mode_x = EC_READ_U8(domainInput_pd + modes_of_operation_display_x);
        cur_statuswd_x = EC_READ_U16(domainInput_pd + statwd_x);
        EC_WRITE_U8(domainOutput_pd + operation_mode_y, 6);
        EC_WRITE_U8(domainOutput_pd + origin_mode_y, 35);
        cur_mode_y = EC_READ_U8(domainInput_pd + modes_of_operation_display_y);
        cur_statuswd_y = EC_READ_U16(domainInput_pd + statwd_y);
        AxisProcessESM(gSysRunning.m_gWorkStatus);

        if(x_ready_origin == 1 && y_ready_origin == 1) {
            static int delay_time_origin;
            delay_time_origin++;
            printf("X_curpos = %d \t", EC_READ_S32(domainInput_pd + actpos_x));
            printf("Y_curpos = %d \n", EC_READ_S32(domainInput_pd + actpos_y));
            if (delay_time_origin >= 1000) {
                if (((cur_statuswd_x & STATUS_SERVO_ENABLE_BIT) != 0) &&
                    ((cur_statuswd_y & STATUS_SERVO_ENABLE_BIT) != 0)) {
                    gSysRunning.m_gWorkStatus = SYS_WORKING_OP_MODE;
                    printf(" igh_ESM SYS_WORKING_OP_MODE \n");
                }
            }
        }
        break;
    case SYS_WORKING_OP_MODE:
        EC_WRITE_U8(domainOutput_pd + operation_mode_x, CSP_MODE);
        cur_mode_x = EC_READ_U8(domainInput_pd + modes_of_operation_display_x);
        cur_statuswd_x = EC_READ_U16(domainInput_pd + statwd_x);
        EC_WRITE_U8(domainOutput_pd + operation_mode_y, CSP_MODE);
        cur_mode_y = EC_READ_U8(domainInput_pd + modes_of_operation_display_y);
        cur_statuswd_y = EC_READ_U16(domainInput_pd + statwd_y);
        AxisProcessESM(gSysRunning.m_gWorkStatus);
        if (((cur_statuswd_x & STATUS_SERVO_ENABLE_BIT) != 0) &&
            ((cur_statuswd_y & STATUS_SERVO_ENABLE_BIT) != 0)) {
            gSysRunning.m_gWorkStatus = SYS_WORKING_IDLE_STATUS;
            printf(" igh_ESM SYS_WORKING_IDLE_STATUS \n");
        }
        break;
    default:
        // 读取两轴当前位置
        cur_statuswd_x = EC_READ_U16(domainInput_pd + statwd_x);
        cur_statuswd_y = EC_READ_U16(domainInput_pd + statwd_y);

        if ((cycle_counter % 100) == 0) {
            int readx = EC_READ_S32(domainInput_pd + actpos_x);
            int ready = EC_READ_S32(domainInput_pd + actpos_y);
            printf("X_curpos = %d \t", readx);
            printf("Y_curpos = %d \n", ready);
            pthread_mutex_lock(&pos_mutex);
            curpos_x = readx;
            curpos_y = ready;
            pthread_mutex_unlock(&pos_mutex);
        }

        // 1ms 控制：滤波 + PID，基于绝对目标位置直接写 target position
        {
            // 读取最新命令
            double cmd_pan = 0.0, cmd_tilt = 0.0, cmd_speed = 100.0;
            int have_cmd = 0;
            pthread_mutex_lock(&cmd_mutex);
            if (cmd_available) {
                cmd_pan = recv_pan_cmd;
                cmd_tilt = recv_tilt_cmd;
                cmd_speed = recv_speed_cmd;
                have_cmd = 1;
            }
            pthread_mutex_unlock(&cmd_mutex);

            if (have_cmd) {
                // 获取当前实际角度
                int act_x = EC_READ_S32(domainInput_pd + actpos_x);
                int act_y = EC_READ_S32(domainInput_pd + actpos_y);
                double cur_pan = pulses_to_angle_deg(act_x, 0);
                double cur_tilt = pulses_to_angle_deg(act_y, 1);

                // 平滑目标
                // double target_pan = filter_x.update(cmd_pan);
                // double target_tilt = filter_y.update(cmd_tilt);
                double target_pan = cmd_pan;
                double target_tilt = cmd_tilt;
                // PID 计算（dt = 1ms）
                double out_pan = pid_x.compute(target_pan, cur_pan, PERIOD_S);
                double out_tilt = pid_y.compute(target_tilt, cur_tilt, PERIOD_S);

                // 下一时刻的目标角度（绝对）
                double next_pan = cur_pan + out_pan;
                double next_tilt = cur_tilt + out_tilt;

                // 角度 -> 脉冲并写入
                int target_pulses_x = angle_deg_to_pulses(next_pan, 0);
                int target_pulses_y = angle_deg_to_pulses(next_tilt, 1);
                EC_WRITE_S32(domainOutput_pd + targetpos_x, target_pulses_x);
                EC_WRITE_S32(domainOutput_pd + targetpos_y, target_pulses_y);

                static int print_cycle = 0;
                print_cycle++;
                if ((print_cycle % 100) == 0) {
                    printf("[RT_CTRL] cmd(%.1f,%.1f) cur(%.2f,%.2f) out(%.2f,%.2f)\n",
                           cmd_pan, cmd_tilt, cur_pan, cur_tilt, out_pan, out_tilt);
                }
            }
        }

#if STEP_TEST_MODE
        StepMoveTest(1, 100, 0);
        StepMoveTest(1, 100, 1);
#endif

        break;
    }
    // write application time to master
    ecrt_master_application_time(master, system_time_ns());
    ecrt_master_sync_reference_clock(master);
    ecrt_master_sync_slave_clocks(master);
    // send process data
    ecrt_domain_queue(domainServoOutput);
    ecrt_domain_queue(domainServoInput);
    ecrt_master_send(master);
}

/****************************************************************************/
void *InterpolationThread(void *arg)
{
    struct timespec wait, previous;
    clock_gettime(CLOCK_REALTIME, &previous);
    wait = previous;
    while (run) {
        wait.tv_nsec += PERIOD_NS; // 1ms
        if (wait.tv_nsec >= NSEC_PER_SEC) {
            wait.tv_sec++;
            wait.tv_nsec -= NSEC_PER_SEC;
        }
        clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &wait, NULL);
        DriverEtherCAT();
    }
    return NULL;
}

/****************************************************************************
 * Signal handler
 ***************************************************************************/
void signal_handler(int sig)
{
    run = 0;
}

/****************************************************************************
 * Main function
 ***************************************************************************/
int main(int argc, char *argv[])
{
    int ret;
    signal(SIGTERM, signal_handler);
    signal(SIGINT, signal_handler);
    if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
        perror("mlockall failed");
        return -1;
    }

    gSysRunning.m_gWorkStatus = SYS_WORKING_POWER_ON;
    if (gSysRunning.m_gWorkStatus == SYS_WORKING_POWER_ON) {
        ActivateMaster();
        gSysRunning.m_gWorkStatus = SYS_WORKING_SAFE_MODE;
        printf("SYS_WORKING_SAFE_MODE\n");
    }

    // 创建实时线程
    pthread_attr_t attr;
    struct sched_param param;
    pthread_attr_init(&attr);
    pthread_attr_setschedpolicy(&attr, SCHED_FIFO);
    param.sched_priority = 99;
    pthread_attr_setschedparam(&attr, &param);
    pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED);
    ret = pthread_create(&InterpolationTask, &attr, InterpolationThread, NULL);
    if (ret) {
        fprintf(stderr, "Failed to create task: %s\n", strerror(ret));
        return -1;
    }

#if !STEP_TEST_MODE
    // 启动 UDP 接收线程（用于接收来自 Python 的 MOVE 指令）
    pthread_t udp_thread;
    if (pthread_create(&udp_thread, NULL, udp_thread_func, NULL) != 0) {
        perror("create udp thread failed");
    }
#endif

    printf("Starting InterpolationTask...\n");
    while (run) {
        usleep(50000);
    }
    printf("Deleting realtime InterpolationTask task...\n");
    pthread_join(InterpolationTask, NULL);

#if !STEP_TEST_MODE
    // 等待 UDP 线程退出
    pthread_join(udp_thread, NULL);
#endif

    ReleaseMaster();
    return 0;
}
